extern crate nalgebra as na;
use na::*;
use rand::Rng;
use rayon::vec;
use statrs::distribution;
use std::f64::consts::PI;
///原文链接：https://blog.csdn.net/weixin_62891964/article/details/126839894
///标准D-H参数（SDH）  z是关节旋转轴,下一个x_i垂直相交上一个z_i-1
///
///关节角theta_i：绕轴z_i-1，x_i-1旋转到x_i 的角度；[-pi,pi]
///关节距离d_i     从轴z_i-1，x_i-1移动到xi 的距离； zi-1正方向

///连杆长度a_i：    从轴𝑥𝑖，z_i-1移动到zi的距离； xi正方向

///连杆扭角αlpha_𝑖：绕轴𝑥𝑖，z_i-1旋转到zi的角度 [-pi,pi]

#[derive(Debug)]
pub struct DH {
    pub theta: f64,
    pub alpha: f64,
    pub a: f64,
    pub d: f64,
}

/*

*/
impl DH {
    pub fn trans_mat(&self) -> Matrix4<f64> {
        let st = self.theta.sin();
        let ct = self.theta.cos();
        let sa = self.alpha.sin();
        let ca = self.alpha.cos();
        let tr = Matrix4::new(
            ct,
            -st * ca,
            st * sa,
            self.a * ct,
            st,
            ct * ca,
            -ct * sa,
            self.a * st,
            0.,
            sa,
            ca,
            self.d,
            0.,
            0.,
            0.,
            1.,
        )
        .transpose();
        tr
    }
    pub fn basic_jacobian(&self, trans_prev: Matrix4<f64>, xyz: Matrix3x1<f64>) -> Vec<f64> {
        let pos_prev = Matrix3x1::new(trans_prev[(3, 0)], trans_prev[(3, 1)], trans_prev[(3, 2)]);
        let z_prev = Vector3::new(trans_prev[(2, 0)], trans_prev[(2, 1)], trans_prev[(2, 2)]);
        let dif = xyz - pos_prev;
        let mut c = z_prev.cross(&dif).as_slice().to_owned();
        c.push(z_prev.x);
        c.push(z_prev.y);
        c.push(z_prev.z);
        c
    }
}
/*
    def basic_jacobian(trans_prev, ee_pos):
        print ("arg==",trans_prev,ee_pos)
        pos_prev = np.array(
            [trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
        z_axis_prev = np.array(
            [trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])

        basic_jacobian = np.hstack(
            (np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))

        print (111,pos_prev,z_axis_prev,basic_jacobian)
        print (222,np.cross(z_axis_prev, ee_pos - pos_prev))
        return basic_jacobian
*/
fn xyz(m: &Matrix4<f64>) -> (f64, f64, f64) {
    // !!!!!!!!!!! rs 的行列索引和numpy反着来的
    let x = m[(3, 0)];
    let y = m[(3, 1)];
    let z = m[(3, 2)];
    (x, y, z)
}
fn euler_angle(m: &Matrix4<f64>) -> (f64, f64, f64) {
    let mut alpha = m[(2, 1)].atan2(m[(2, 0)]);
    if !(alpha >= -PI / 2.0 && alpha <= PI / 2.0) {
        alpha = m[(2, 1)].atan2(m[(2, 0)]) + PI;
    }
    if !(alpha >= -PI / 2.0 && alpha <= PI / 2.0) {
        alpha = m[(2, 1)].atan2(m[(2, 0)]) - PI;
    }
    let beta_y = m[(2, 0)] * alpha.cos() + m[(2, 1)] * alpha.sin();
    let beta_x = m[(2, 2)];
    let beta = beta_y.atan2(beta_x);
    let gama_y = -m[(0, 0)] * alpha.sin() + m[(0, 1)] * alpha.cos();
    let gama_x = -m[(1, 0)] * alpha.sin() + m[(1, 1)] * alpha.cos();
    let gama = gama_y.atan2(gama_x);
    (alpha, beta, gama)
}
pub struct DHS(Vec<DH>);
impl DHS {
    pub fn from_root_to_end(&self) -> Matrix4<f64> {
        let mut m = Matrix4::<f64>::identity();
        for i in &self.0 {
            let tr = i.trans_mat();
            // !!!!!!!!!! trans is before
            m = tr * m;
        }
        m
    }
    pub fn forward(&self) -> Vec<f64> {
        let m = self.from_root_to_end();
        let p = xyz(&m);
        let euler = euler_angle(&m);
        vec![p.0, p.1, p.2, euler.0, euler.1, euler.2]
    }
    pub fn jacobian(&self) -> Vec<Vec<f64>> {
        let cur = self.forward();
        let xyz = Matrix3x1::new(cur[0], cur[1], cur[2]);
        let mut jac_mat_list = Vec::new();
        let mut trans = Matrix4::<f64>::identity();
        for link in &self.0 {
            let jac = link.basic_jacobian(trans, xyz);
            jac_mat_list.push(jac);
            let tr = link.trans_mat();
            trans = tr * trans;
        }
        jac_mat_list
    }
    /// one round
    pub fn inverse(&mut self, tar_pos: &Vec<f64>) -> Vec<f64> {
        let cur = self.forward();
        let dif: Vec<f64> = tar_pos.into_iter().zip(cur).map(|v| v.0 - v.1).collect();
        let jac = self.jacobian();
        let trans = self.from_root_to_end();
        let eu = euler_angle(&trans);
        dbg!(jac, eu);
        let l_zyz = vec![
            0.,
            eu.0.sin(),
            eu.0.cos() * eu.1.sin(),
            0.,
            eu.0.cos(),
            eu.0.sin() * eu.1.sin(),
            1.,
            0.,
            eu.1.cos(),
        ];
        let K_zyz = Matrix3::from_vec(l_zyz);
        dbg!(K_zyz);
        vec![0.]
    }
}

#[test]
fn test_f_kina() {
    let dh1 = DH {
        theta: 0.,
        alpha: -PI / 2.,
        a: 0.1,
        d: 0.,
    };
    let dh2 = DH {
        theta: 0.,
        alpha: PI / 2.,
        a: 0.,
        d: 0.,
    };
    let dh3 = DH {
        theta: 0.,
        alpha: 0.,
        a: 0.,
        d: 0.,
    };
    let target = vec![
        0.4615526833399779,
        -0.10358889419961226,
        -0.4060298761019099,
        -0.44900959684297026,
        -0.04790287716219266,
        0.17072276427146582,
    ];
    let l = vec![dh1, dh2, dh3];
    let mut dhs = DHS(l);
    let res = dhs.inverse(&target);
    dbg!(111111, res);
}
